% Perform simulations for a 2 vehicle chain using data as input
% clear; clc;
% close all;

%% Parameters
% dataset to use
% 4 vehicles, 3 CHV + 1 CAV
% load('20170727_202308_4_vehicles');
% kcav=4;
% alpha=par(1);
% ki = par(2);
% % beta=[0.38,0.3,0.3];
% % gamma=[0.4,0,0];
% beta=[par(3),par(4),par(5)];
% gamma=[par(6),par(7),par(8)];
% % 8 vehicles, 6 CHV + 1 CAV + 1 CHV
load('20170730_114049_8_vehicles');
load('parameters_6_predecessors.mat');
% load('parameters_4381_hgo41_vmax28.mat');
kcav=7;
alpha=par(1);
ki = par(2);
% beta= [0.2,0.3,0.3,0,0,0];
% gamma=[0.4,0,0,0,0,0];
beta= [par(3),par(4),par(5),par(6),par(7),par(8)];
gamma=[par(9),par(10),par(11),par(12),par(13),par(14)];
% % fixed parameters for CAV
sigma=0.6;
hst=5;
hgo=55;
vmax=30;
amin=7;
amax=3;
betasum=sum(beta);

% parameters for safe and non-conservative bounds
kappa_min=0.35;
kappa_max=1.5;
hst_min=3;
hst_max=10;

% number of measured vehicles
veh_num=length(time);
% vehicle number of CAV: kcav

% range policy and saturations for CAV
% V=@(h)vmax*(hgo<=h) + vmax*(h-hst)/(hgo-hst).*(hst<h & h<hgo); % linear range policy
% V=@(h)vmax.*(hgo<=h) + (vmax*(2*hgo - hst - h).*(h-hst)/(hgo-hst)^2).*(hst<h & h<hgo); % quadratic
% V=@(h)vmax.*(hgo<=h) + (vmax*(3*hgo - hst - h).*(h-hst).^2/(hgo-hst)^3).*(hst<h & h<hgo); % cubic
V=@(h)vmax.*(hgo<=h) + (vmax*0.5*(1 - cos(pi*(h-hst)/(hgo-hst)))).*(hst<h & h<hgo); % paper
W=@(vL)vmax*(vmax<=vL)+vL.*(vL<vmax);
sat=@(u)(u<-amin).*(-amin)+(-amin<=u & u<=amax).*u+(amax<u).*amax;

% control input for CAV
u=@(h,v,z,vL,vLdot)alpha*V(h)+ki*z-(alpha+betasum)*v+beta*W(vL)+gamma*vLdot;

% initial conditions for CAV
h0=hdwy{kcav}(1);
v0=vel{kcav}(1);
xinit=@(t)[h0;v0;0];

% simulation time
t0=min(vertcat(time{:}));
tend=max(vertcat(time{:}));
deltat=min(diff(time{1}));
tsim=(t0:deltat:tend).';        

% number of preceding CHVs that the CAV may respond to
chv_num=length(beta);
% preceding CHVs' velocity from data
for kL=1:chv_num
    vLead(:,kL)=interp1(time{kcav-kL},vel{kcav-kL},tsim,'linear','extrap');
    vLeaddelay(:,kL)=interp1(tsim,vLead(:,kL),tsim-sigma,'linear','extrap');
    vLeaddot(:,kL)=interp1(time{kcav-kL},acc{kcav-kL},tsim,'linear','extrap');
    vLeaddotdelay(:,kL)=interp1(tsim,vLeaddot(:,kL),tsim-sigma,'linear','extrap');
end
vL=@(t)vLead(t==tsim,:).';
vLdelay=@(t)vLeaddelay(t==tsim,:).';
vLdotdelay=@(t)vLeaddotdelay(t==tsim,:).';
select=@(vL,kL)vL(kL);  % function for selecting element from vector

% title to put on figure
problem='CCC using measurement data';
% list of parameters to put on figure
parlist=['\alpha=',num2str(alpha,'%3.2f'),' [1/s]   ',...
         '\beta=[',regexprep(num2str(beta),'\s+',', '),'] [1/s]   '...
         '\sigma=',num2str(sigma,'%3.2f'),' [s]'];
% legend to put on figure
vehtype=repmat({'measured CHV'},1,veh_num);
vehtype{kcav}='measured CAV';
vehlegend=horzcat(vehtype,{'simulated CAV'});

% load('u_of_t_profile_6_predecessors.mat')
% cont_input = control_input(:,1);
% cont_time = control_input(:,2);
% u_of_time = @(t)interp1(cont_time,cont_input,t,'linear','extrap');
%% Simulation
% right-hand side of equations
model=@(t,x,xdelay)[select(vL(t),1)-x(2);
                    sat(u(xdelay(1),xdelay(2),xdelay(3),vLdelay(t),vLdotdelay(t)));
                    V(x(1))-x(2)];
% model=@(t,x,xdelay)[select(vL(t),1)-x(2);
%                     u_of_time(t);
%                     V(x(1))-x(2)];
% perform simulation
x=ddeab4(@(t,x,xdelay)model(t,x,xdelay),sigma,xinit,tsim);

% extract headway and velocity
hdwysim=x(1,:).';
velsim =x(2,:).';

% check safe and non-conservative bounds
check_bounds(hdwysim,velsim,kappa_min,kappa_max,hst_min,hst_max);

% calculate accelearation
accsim=sgolayfilt(diff(velsim)/deltat,3,21);
accsim=[accsim(1);accsim];

%% Control input as a function of time
% % % hdwysim delay
% % % hdwysimdelay=interp1(tsim,hdwysim,tsim-sigma,'linear','extrap');
% % % hdwysimdelay(1:6,1) = ones(6,1)*h0;
% 
% hdwysimdelay= [ones(7,1)*h0;hdwysim(8:end,1)];
% 
% % % velsim delay
% % % velsimdelay=interp1(tsim,velsim,tsim-sigma,'linear','extrap');
% % % velsimdelay(1:6,1) = ones(6,1)*v0;
% velsimdelay= [ones(7,1)*v0;velsim(8:end,1)];
% % test1 = alpha*hdwysimdelay(tsim);
% % test2 = -(alpha+betasum)*velsimdelay(tsim);
% % test3 = beta*vLdelay(tsim);
% % test4 = gamma*vLdotdelay(tsim);
% 
% u_of_t =@(t) sat(u(hdwysimdelay(t),velsimdelay(t),0*t',vLdelay(t),vLdotdelay(t)));
% control_input = [u_of_t(tsim)' tsim];
% save('u_of_t_profile_6_predecessors.mat','control_input');
%%
% calculate energy consumption
gamma=0.01;         % [-] tyre rolling resistance coefficient
g=9.81;             % [m/s^2] gravitatioinal constant
a=gamma*g;          % [m/s^2]
Cd=0.34;            % [-] air drag coefficient
A=2.32;             % [m^2] frontal area
rho=1.23;           % [kg/m^3] air density at 25 degree
k=0.5*Cd*rho*A;     % [kg/m]
m=1770;             % [kg] mass of the vehicle
c=k/m;              % [1/m]
enconssim=cumsum(max(accsim+a+c*velsim.^2,0).*velsim*deltat);
encon = enconssim(end)

% vcond = (hdwysim - hst_max)*kappa_min;
% vsafe = (hdwysim - hst_min)*kappa_max;
% vmid  = hdwysim*(kappa_max+kappa_min)/2 - (hst_min*kappa_max + hst_max*kappa_min)/2;
% 
% delta_V_cond = sum(velsim - vcond)
% delta_V_safe = sum(velsim - vsafe)
% delta_V_mid = abs(sum(velsim - vmid))

%% Objective function
% Extract only the velocities that exceed the safe and cond boundary 
sum_safeBound_exeeded = 0;
sum_condBound_exeeded = 0;
sum_safeBound_obj = 0;
sum_condBound_obj = 0;
for i = 1:length(velsim)
    vcond = (hdwysim(i) - hst_max)*kappa_min;
    vsafe = (hdwysim(i) - hst_min)*kappa_max;
    % vsim exceeds safe bound? (vsim will be greate than vcond)
    if velsim(i) >= vsafe
        sum_safeBound_obj = sum_safeBound_exeeded + (velsim(i) - vsafe);
    % vsim exceeds cond bound? (vsim will be less than vcond)    
    elseif velsim(i) <= vcond
        sum_condBound_obj = sum_condBound_exeeded + (vcond - velsim(i));
    end
end
% sum_safeBound_obj
% sum_condBound_obj
% % vcond = (hdwysim - hst_max)*kappa_min;
% % vsafe = (hdwysim - hst_min)*kappa_max;
% % vmid  = hdwysim*(kappa_max+kappa_min)/2 - (hst_min*kappa_max + hst_max*kappa_min)/2;
% % delta_V_cond = sum(velsim - vcond);
% % delta_V_safe = sum(velsim - vsafe);
% % delta_V_mid  = sum(abs(velsim - vmid));

% obj = enCon + 1e5/delta_V_cond + 1e5/delta_V_safe;
% obj = enCon + delta_V_mid*0.3; % Energy consumption and bring velocities closer to the middle

% obj = enCon - delta_V_cond + delta_V_safe;

% objective = energy consumption + sum of velocites that exceed the bounds
% obj = enCon + sum_safeBound_obj + sum_condBound_obj;
% sum(accsim.^2)
% sum((V(hdwysim) - velsim).^2)
% sum((diff(hdwysim)/deltat).^2)

%% Plot of solution
% plot velocity vs time
figure(1); clf; hold on; box on;
LL=zeros(veh_num+1,1);
% plot measured velocity
for kk=1:veh_num
%    plot(time{kk},vel{kk},'Linewidth',1.5,'Color',colours(kk,:));
   LL(kk)=plot(timeLoss{kk},velLoss{kk},'Linewidth',1.5,'Color',colours(kk,:));
   plot(timeRec{kk},velRec{kk},'Linewidth',1.5,'Color',colours(kk,:),'LineStyle','none','Marker','x');
end
% plot speed of simulated CAV
LL(end)=plot(tsim,velsim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
xlabel('Time [s]');ylabel('Speed [m/s]');
title(['Simulation results of ',problem,10,parlist]);
legend(LL,vehlegend,'Location','northwest');

% plot acceleration vs time
figure(2); clf; hold on; box on;
LL=zeros(veh_num+1,1);
% plot measured acceleration
for kk=1:veh_num
%    plot(time{kk},acc{kk},'Linewidth',1.5,'Color',colours(kk,:));
   LL(kk)=plot(timeLoss{kk},accLoss{kk},'Linewidth',1.5,'Color',colours(kk,:));
   plot(timeRec{kk},accRec{kk},'Linewidth',1.5,'Color',colours(kk,:),'LineStyle','none','Marker','x');
end
% plot acceleration of simulated vehicle
LL(end)=plot(tsim,accsim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
xlabel('Time [s]');ylabel('Acceleration [m/s^2]');
title(['Simulation results of ',problem,10,parlist]);
legend(LL,vehlegend,'Location','southwest');

% plot headway vs time
figure(3); clf; hold on; box on;
LL=zeros(veh_num+1,1);
% plot measured headway
for kk=1:veh_num
%    plot(tCom{kk},hdwy{kk},'Linewidth',1.5,'Color',colours(kk,:));
   LL(kk)=plot(tComLoss{kk},hdwyLoss{kk},'Linewidth',1.5,'Color',colours(kk,:));
   plot(tComRec{kk},hdwyRec{kk},'Linewidth',1.5,'Color',colours(kk,:),'LineStyle','none','Marker','x');
end
% plot headway of simulated vehicle
LL(end)=plot(tsim,hdwysim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
xlabel('Time [s]');ylabel('Headway [m]');
title(['Simulation results of ',problem,10,parlist]);
legend(LL,vehlegend,'Location','northwest');

% plot energy consumption vs time
figure(4); clf; hold on; box on;
LL=zeros(veh_num+1,1);
% plot measured energy consumption
for kk=1:veh_num
%    plot(time{kk},encons{kk},'Linewidth',1.5,'Color',colours(kk,:));
   LL(kk)=plot(timeLoss{kk},enconsLoss{kk},'Linewidth',1.5,'Color',colours(kk,:));
   plot(timeRec{kk},enconsRec{kk},'Linewidth',1.5,'Color',colours(kk,:),'LineStyle','none','Marker','x');
end
% plot energy consumption of simulated vehicle
LL(end)=plot(tsim,enconssim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
xlabel('Time [s]');ylabel('Energy consumption [J/kg]');
title(['Simulation results of ',problem,10,parlist]);
legend(LL,vehlegend,'Location','northwest');

%% Evaluation of frequency response
% for FFT, consider only those times when vehicles were moving
% when the CAV's speed (and possibly its predecessor's speed) exceeded vmin
vmin=1;
% get time interval (t1<=t<=t2) corresponding to moving
t1=time{kcav}(find(vel{kcav}>=vmin & ...
    interp1(time{kcav-chv_num},vel{kcav-chv_num},time{kcav})>=vmin,1,'first'));
t2=time{kcav}(find(vel{kcav}>=vmin,1,'last'));

% pick time vector to which data is interpolated before doing FFT
tFFT=tsim(t1<=tsim & tsim<=t2);
% get number of data points
n=length(tFFT);
% get sampling frequency and discrete frequency values
fs=1/deltat;
freq=(0:floor(n/2))/n*fs;

% frequency content of measured velocity
velFFT=cell(size(vel));
for kk=1:veh_num
    % interpolate velocities to the same time vector
    velinterp=interp1(time{kk},vel{kk},tFFT,'linear','extrap');
    % calculate FFT for velocity fluctuations
    velFFT{kk}=abs(fft(velinterp-mean(velinterp))/n);
    % take twice the first half of the FFT
    velFFT{kk}=velFFT{kk}(1:floor(n/2+1));
    velFFT{kk}(2:end)=2*velFFT{kk}(2:end);
    % filter the FFT for smoother results
    velFFT{kk}=abs(sgolayfilt(velFFT{kk},3,31));
end

% frequency content of simulated velocity
velinterp=interp1(tsim,velsim,tFFT,'linear','extrap');
velsimFFT=abs(fft(velinterp-mean(velinterp))/n);
velsimFFT=velsimFFT(1:floor(n/2+1));
velsimFFT(2:end)=2*velsimFFT(2:end);
velsimFFT=abs(sgolayfilt(velsimFFT,3,31));

% plot frequency content
figure(5); clf; box on;
for kk=1:veh_num
    semilogy(freq,velFFT{kk},'Linewidth',1.5,'Color',colours(kk,:));
    hold on;
end
semilogy(freq,velsimFFT,'Linewidth',2.5,'Color','r');
xlim([0,1]);
% ylim([0.001,1]);
xlabel('Frequency [Hz]');ylabel('Velocity [m/s]');
title(['Frequency content of velocity fluctuations',10,parlist]);
legend(vehlegend,'Location','northeast');

% plot frequency response wrt the head vehicle of the full group
figure(6); clf; box on;
for kk=2:veh_num
    semilogy(freq,velFFT{kk}./velFFT{1},'Linewidth',1.5,'Color',colours(kk,:));
    hold on;
end
semilogy(freq,velsimFFT./velFFT{1},'Linewidth',2.5,'Color','r');
xlim([0,1]);
% ylim([0.1,10]);
xlabel('Frequency [Hz]');ylabel('Amplification');
title(['Frequency repsonse w.r.t. the head vehicle of the full group',10,parlist]);
legend(vehlegend(2:end),'Location','northeast');

% plot frequency response wrt head vehicle of the CHVs the CAV responds to
kfirst=kcav-find(beta~=0,1,'last');
figure(7); clf; box on;
for kk=kfirst+1:veh_num
    semilogy(freq,velFFT{kk}./velFFT{kfirst},'Linewidth',1.5,'Color',colours(kk,:));
    hold on;
end
semilogy(freq,velsimFFT./velFFT{kfirst},'Linewidth',2.5,'Color','r');
xlim([0,1]);
% ylim([0.1,10]);
xlabel('Frequency [Hz]');ylabel('Amplification');
title(['Frequency repsonse w.r.t. the head of the CHVs the CAV responds to',10,parlist]);
legend(vehlegend(kfirst+1:end),'Location','northeast');

% string stability index
Cs=sum(max(velsimFFT./velFFT{1}-1,0)*fs/n);

%% Evaluation of time to collision
% calculate time to collision
ttc=cell(size(vel));
for kk=2:veh_num
    ttc{kk}=interp1(tCom{kk},hdwy{kk},time{kk})./...
            (vel{kk}-interp1(time{kk-1},vel{kk-1},time{kk}));
    ttc{kk}(ttc{kk}<0)=nan;
end
ttcsim=hdwysim./(velsim-interp1(time{kcav-1},vel{kcav-1},tsim));
ttcsim(ttcsim<0)=nan;

% plot time to collision vs time
figure(8); clf; hold on; box on;
for kk=2:veh_num
   plot(time{kk},ttc{kk},'Linewidth',1.5,'Color',colours(kk,:));
end
plot(tsim,ttcsim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
ylim([0,20]);
xlabel('Time [s]');ylabel('Time to collision [s]');
title(['Simulation results of ',problem,10,parlist]);
legend(vehlegend(2:end),'Location','southeast');

% time to collision index
ttccr=2;
Ct=sum(max(ttccr-ttcsim,0)*deltat);

%% Evaluation of headway error
% calculate headway error (difference from desired headway)
Vinv=@(v)hst+v/vmax*(hgo-hst);  % valid for 0<v<vmax only
hdwyerr=cell(size(vel));
for kk=2:veh_num
    hdwyerr{kk}=hdwy{kk}-interp1(time{kk},Vinv(vel{kk}),tCom{kk});
end
hdwyerrsim=hdwysim-Vinv(velsim);

% plot headway error vs time
figure(9); clf; hold on; box on;
for kk=2:veh_num
   plot(tCom{kk},hdwyerr{kk},'Linewidth',1.5,'Color',colours(kk,:));
end
plot(tsim,hdwyerrsim,'Linewidth',2.5,'Color','r');
xlim([t0,tend]);
xlabel('Time [s]');ylabel('Headway error [m]');
title(['Simulation results of ',problem,10,parlist]);
legend(vehlegend(2:end),'Location','northwest');

%% Plot bounds and data on (h,v) plane for the simulated controller
figure(10); clf; hold on; box on;
plot(hdwysim,velsim,'Linewidth',2)
hold on
h = -10:0.01:100;
vsafe = (h-hst_min)*kappa_max;
vncon = (h-hst_max)*kappa_min;
plot(h,vsafe,'r','Linewidth',2)
hold on
plot(h,vncon,'r','Linewidth',2)
xlim([0 70])
ylim([0 30])
xlabel('Headway h','Fontsize',16)
ylabel('Velocity v','Fontsize',16)
title(['Simulated controller for ',num2str(veh_num),'-vehicle dataset'],'Fontsize',16)